/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *
 * FILENAME:
 * gpsWaypoint.h
 *
 * PURPOSE:
 *  Input and output of GPS waypoint lists as defined by CloudCap Tech
 * for the Piccolo.
 *
 * CREATED:
 * 02/2005 by Cory Dixon
 *
 ***********************************************************************/

#ifndef __cplusplus
#       error Must use C++ for the type gpsWaypointFile.
#endif

#ifndef _GPSWAYPOINT_H
#define _GPSWAYPOINT_H

#include <stdio.h>
#include <iostream>
#include <iomanip>

#include "String.h"
#include "commandStruct.h"
 
#define FLIGHTPLAN_EXT_STRING ".fp"

using namespace std;

// ----------
// we "borrowed" from cloud cap tech. Thanks!
// ----------
#define NUM_WP_POINTS 100  //!< The number of waypoints that the avionics holds
// Number of bytes required to hold 1 bit for each waypoint 
#define WP_MASK_BYTES (((NUM_WP_POINTS-1)/8)+1)

//////////////////////////////////////////////////////////////////////
//
//////////////////////////////////////////////////////////////////////
typedef struct _GPSWaypoint_t
{
	uint8_t index;
	uint8_t Next;
	float Lat;
	float Lon;
	float Alt;
	float WindInterval;
	bool Preturn;
	bool Lights;
	bool Slope;
	bool Land;
	bool Parachute;
	bool Drop;
	bool Camera;
	bool OrbitDirection;
	bool Hover;
	float OrbitRadius;
	float OrbitTime;
	int   User;
	float OrbitAboveAlt;
	float OrbitBelowAlt;

	unsigned int validElements;

	/* Determine if the passed flight plan point is valid.  This is based upon
	 *    the index of the next waypoint. */
	bool IsPointValid() 
	{
		if(Next < NUM_WP_POINTS)
			return true;
		return false;
	};

	/* Initialize waypoint */
	_GPSWaypoint_t() {
		clear();
	}

	void clear() {
		index = 0xFF;
		Next = 0xFF;
		Lat=0;
		Lon=0;
		Alt=0;
		WindInterval=0;
		Preturn=0;
		Lights=0;
		Slope=0;
		Land=0;
		Parachute=0;
		Drop=0;
		Camera=0;
		OrbitDirection=0;
		Hover=0;
		OrbitRadius=0;
		OrbitTime=0;
		User=0;
		OrbitAboveAlt=0;
		OrbitBelowAlt=0;

		validElements = 0;
	};
	
	// friend functions
	friend ostream& operator << (ostream& ostrm, _GPSWaypoint_t& p){
		ostrm 
			<< (int)p.index << " "
			<< (int)p.Next << " "
			<< setprecision (9) 
			<< p.Lat << " "
			<< p.Lon << " "
			<< p.Alt << " ";
		if( p.validElements >= 17 ) {
		ostrm
			<< p.WindInterval << " "
			<< p.Preturn << " "
			<< p.Lights << " "
			<< p.Slope << " "
			<< p.Land << " "
			<< p.Parachute << " "
			<< p.Drop << " "
			<< p.Camera << " "
			<< p.OrbitDirection << " "
			<< p.OrbitRadius << " "
			<< p.OrbitTime << " "
			<< p.User << " ";
		}
		if( p.validElements >= 20 ) {
		ostrm
			<< p.OrbitAboveAlt << " "
			<< p.OrbitBelowAlt << " "
			<< p.Hover << " ";
		}

		// reset output format
		return ostrm;
   }
 
	friend istream& operator >> (istream& istrm, _GPSWaypoint_t& point)
	{
		int index, next;
		istrm 
			>> index
			>> next
			>> point.Lat
			>> point.Lon
			>> point.Alt;
		point.index = index;
		point.Next  = next;
			if( point.validElements >= 17 ) {
				istrm >> point.WindInterval
					>> point.Preturn
					>> point.Lights
					>> point.Slope
					>> point.Land
					>> point.Parachute
					>> point.Drop
					>> point.Camera
					>> point.OrbitDirection
					>> point.OrbitRadius
					>> point.OrbitTime
					>> point.User;
			} 
			if( point.validElements >= 20 ) {
				istrm >> point.OrbitAboveAlt
					>> point.OrbitBelowAlt
					>> point.Hover;
			}

		return istrm;
	}

	Waypt_t getWP_t() {

		Waypt_t pt;
		pt.pos.lat  = Lat;
		pt.pos.lon  = Lon;
		pt.pos.alt  = Alt;
		pt.number   = index;
		pt.next     = Next;
		pt.orbitRad = (int16_t) OrbitRadius;
		pt.orbitRad *= ( OrbitDirection == 1 ? -1 : 1 );

		return pt;
	}

} GPSWaypoint_t;


// The flight plan mask is an array of bytes with one bit for each waypoint 
//   thus there are types of WPMask_t mask;
typedef uint8_t WPMask_t[WP_MASK_BYTES];
//   thus there are types of WPPlant_t plan;
typedef GPSWaypoint_t WPlan_t[NUM_WP_POINTS];

//-------------------------------------------------
// Waypoint Mask utility functions
//-------------------------------------------------
int32_t FindFirstWPMaskBitSet(WPMask_t Mask);
int32_t FindNextWPMaskBitSet(WPMask_t Mask, uint32_t i);

void SetWPMaskBit(WPMask_t Mask, uint32_t Point);
void ClearWPMaskBit(WPMask_t Mask, uint32_t Point);
bool GetWPMaskBit(const WPMask_t Mask, uint32_t Point);

void SetAllWPMaskBits(WPMask_t Mask);
void ClearAllWPMaskBits(WPMask_t Mask);

void InvertAllWPMaskBits(WPMask_t Mask);
void InvertAllWPmaskBits_Ex(WPMask_t Dest, const WPMask_t Source);

void WPMaskOR(WPMask_t Mask1, const WPMask_t Mask2);
void WPMaskOR_Ex(WPMask_t Return, const WPMask_t Mask1, const WPMask_t Mask2);
void WPMaskAND(WPMask_t Mask1, const WPMask_t Mask2);
void WPMaskAND_Ex(WPMask_t Return, const WPMask_t Mask1, const WPMask_t Mask2);

int32_t CompareWPMask(const WPMask_t First, const WPMask_t Second);
void    CopyWPMask(const WPMask_t Source, WPMask_t Destination);

//-------------------------------------------------
// Waypoint Plan class
//-------------------------------------------------
class GPSWaypointPlan
{

public:
	GPSWaypointPlan();
	~GPSWaypointPlan();

	bool read_from_file(String fname);
	bool read_from_cmd(uint8_t* buf);
	bool save_to_file(String fname, uint8_t start);

	int           numPoints() {return numWaypoints; }
	bool          add_waypoint(GPSWaypoint_t &point);
	GPSWaypoint_t get_waypoint(uint8_t ind);
	bool          set_waypoint(GPSWaypoint_t &point);

	bool   clear_plan(int ind);
	void   clear_list();
	//void   remove_last_waypoint();
	//void   close_loop(int ind);

	int32_t find_closest(double lon, double lat);
	// find the very begining of the flight plan, or the lowest number 
	int32_t find_first_in_plan(int startInd);

	double  dist_deg2(int wpInd, double lon, double lat);

	//-------------------------------------------------
	// Flight plan utilties
	//-------------------------------------------------
	bool    CheckFlightPlanClosure(uint8_t Start);
	uint8_t GetPointsInPlan(WPMask_t Mask, uint8_t Next);

	int32_t FindPreviousWPPoint(uint8_t Point);
	void    ClearPointsFromPlan(WPMask_t Mask);

//private:
	int     numWaypoints;
	WPlan_t Plan;
};


#endif //_GPSWAYPOINT_H

